#include "PCVectorOp.h"
#include <string.h>
#include "common.h"
#include "lut.h"

int32_t sin(int32_t theta)
{
    int64_t norm = theta;               // m8p24
    int64_t sin_din = norm * pow(2, 6); // m2p30

    /**----------------------------------------------------------*/
    int64_t si = sin_din >> 23; //
    int64_t ci = si + 64;

    int64_t data5 = (sin_din * PI_PAR1) >> 29;          //
    int64_t data0 = ((si * (1 << 23)) * PI_PAR2) >> 29; //

    int64_t d = data5 - data0; //

    int32_t si_adr = si & 255;
    int32_t ci_adr = ci & 255;

    int64_t si_rom = sin_par[si_adr];
    int64_t ci_rom = sin_par[ci_adr];

    int64_t data1 = (si_rom * d) >> 30; //

    int64_t data2 = ci_rom * 2 - data1; //

    int64_t data6 = data2 >> 2; //

    int64_t data3 = (data6 * d) >> 29; //

    double data4 = static_cast<double>(data3);

    int64_t out = si_rom + data4; // m2p30

    int64_t ret = out * 2; // m1p31
    /**----------------------------------------------------------*/
    // m1p31 只有符号位,没有整数位,表示不了sin(pi/2) = 1或者sin(pi/2) = -1, 所以置成极值
    if (ret > INT32_MAX)
        return INT32_MAX;
    else if (ret < INT32_MIN)
        return INT32_MIN;
    else
        return ret;
}

int32_t cos(int32_t theta)
{
    int64_t norm = theta + 0.5 * pow(2, 24); // +M_PI/2 -> m8p24

    // 再一次归到[-1,1] -> [-2^24, 2^24]
    if (norm > pow(2, 24))
    {
        norm = norm - 2 * pow(2, 24); // sin(alpha) = sin(alpha - 2_PI)
    }

    int64_t sin_din = norm * pow(2, 6); // m2p30

    /**----------------------------------------------------------*/
    int64_t si = sin_din >> 23; // p7 移除23位小数(只剩整数部分)
    int64_t ci = si + 64;

    int64_t data5 = (sin_din * PI_PAR1) >> 29;          // p30  (整数+小数)*PI
    int64_t data0 = ((si * (1 << 23)) * PI_PAR2) >> 29; // p30  整数部分*PI

    int64_t d = data5 - data0; //   p30 小数*PI

    int32_t si_adr = si & 255;
    int32_t ci_adr = ci & 255;

    int64_t si_rom = sin_par[si_adr];
    int64_t ci_rom = sin_par[ci_adr];

    int64_t data1 = (si_rom * d) >> 30; // 浮点数

    int64_t data2 = ci_rom * 2 - data1; //

    int64_t data6 = data2 >> 2; //

    int64_t data3 = (data6 * d) >> 29; //

    double data4 = static_cast<double>(data3);

    int64_t out = si_rom + data4; // m2p30

    int64_t ret = out * 2; // m1p31
    /**----------------------------------------------------------*/
    // m1p31 只有符号位,没有整数位,表示不了cos(0) = 1或者cos(pi) = -1或者或者cos(-pi) = -1, 所以置成极值
    if (ret > INT32_MAX)
        return INT32_MAX;
    else if (ret < INT32_MIN)
        return INT32_MIN;
    else
        return ret;
}

int32_t calc_out_size(int8_t cmd_mode, int32_t num, int8_t out_type)
{
    int32_t size = 0;
    for (size_t i = 0; i < 5; i++)
    {
        int8_t mode = CHECK_BIT(cmd_mode, i);
        switch (mode)
        {
        case V_AVG_MODE:
            size += V_RESULT_AVG_SIZE;
            break;
        case V_SUM_MODE:
            size += V_RESULT_SUM_SIZE;
            break;
        case V_MIN_MODE:
            size += V_RESULT_MIN_SIZE;
            break;
        case V_MAX_MODE:
            size += V_RESULT_MAX_SIZE;
            break;
        case V_NORMAL_MODE:
            size += static_cast<int32_t>(pow(2, out_type) * 2 * num);
            break;
        default:
            break;
        }
    }
    return size;
}
/**
 * 功能: 处理向量指令
 * 参数:
 *   - cb: 回调函数，用于处理向量指令
 *   - vp: 向量参数结构体
 *   - u31_cb: U31代码回调函数  (可选)
 *   - u31_code_file: U31代码文件结构体  (可选)
 *   - instruction: 指令名称  (可选)
 *   - case_num: 测试用例编号  (可选)
 * 返回值: 处理是否成功 false:失败 true:成功
 */
bool v_instruction_process(callback cb, VectorParam &vp, u31_code_callback u31_cb,
                           const U31CodeFile &u31_code_file, const char *instruction,
                           int32_t case_num)
{
    vp.out_size = calc_out_size(vp.cmd_mode, vp.num, vp.out_type);
    vp.adr_out = (int8_t *)malloc(vp.out_size);
    if (!vp.adr_out)
    {
        printf("%s vp.adr_out malloc failed!\n", __func__);
        vp.out_size = 0;
        return -1;
    }
    memset(vp.adr_out, 0, vp.out_size);
    cb(vp);

    print_array((int32_t *)vp.adr_out, vp.out_size / 4, "0x%08x");

    if (u31_cb)
        u31_cb(u31_code_file, instruction, case_num, vp);

    FREE(vp.adr_out);
    return true;
}

/**
 * 功能: 获取向量结果
 * 参数:
 *   - cmd_mode: 指令模式
 *   - result: 向量结果结构体
 *   - out_ptr: 输出指针
 * 返回值: 结果大小
 */
int32_t get_vector_result(int8_t cmd_mode, const VectorResult &result, int8_t *out_ptr)
{
    int8_t save_result_order = 0; // 存储顺序为:所有的计算结果、和（64bit）、最大值（32bit）、最小(32bit)、均值(32bit)
    for (size_t i = 0; i < 5; i++)
    {
        int8_t mode = CHECK_BIT(cmd_mode, i);
        switch (mode)
        {
        case V_AVG_MODE:
            SET_BIT(save_result_order, V_SAVE_AVG_INDEX);
            break;
        case V_SUM_MODE:
            SET_BIT(save_result_order, V_SAVE_SUM_INDEX);
            break;
        case V_MIN_MODE:
            SET_BIT(save_result_order, V_SAVE_MIN_INDEX);
            break;
        case V_MAX_MODE:
            SET_BIT(save_result_order, V_SAVE_MAX_INDEX);
            break;
        case V_NORMAL_MODE:
            SET_BIT(save_result_order, V_SAVE_NORMAL_INDEX);
            break;
        default:
            break;
        }
    }

    int32_t ptr_idx = 0;

    for (size_t i = 0; i < 5; i++)
    {
        if (GET_BIT(save_result_order, i))
        {
            switch (i)
            {
            case V_SAVE_NORMAL_INDEX:
            {
                memcpy(out_ptr + ptr_idx, result.data_ptr, result.data_size);
                ptr_idx += result.data_size;
                break;
            }
            case V_SAVE_SUM_INDEX:
            {
                int32_t sum_low = result.sum & 0xFFFFFFFF;
                int32_t sum_high = result.sum >> 32;
                memcpy(out_ptr + ptr_idx, (uint8_t *)&sum_low, V_RESULT_SUM_SIZE / 2); // sum 低4字节
                ptr_idx += V_RESULT_SUM_SIZE / 2;
                memcpy(out_ptr + ptr_idx, (uint8_t *)&sum_high, V_RESULT_SUM_SIZE / 2); // sum 高4字节
                ptr_idx += V_RESULT_SUM_SIZE / 2;
                break;
            }
            case V_SAVE_MAX_INDEX:
            {
                memcpy(out_ptr + ptr_idx, (uint8_t *)&result.max, V_RESULT_MAX_SIZE);
                ptr_idx += V_RESULT_MAX_SIZE;
                break;
            }
            case V_SAVE_MIN_INDEX:
            {
                memcpy(out_ptr + ptr_idx, (uint8_t *)&result.min, V_RESULT_MIN_SIZE);
                ptr_idx += V_RESULT_MIN_SIZE;
                break;
            }
            case V_SAVE_AVG_INDEX:
            {
                memcpy(out_ptr + ptr_idx, (uint8_t *)&result.avg, V_RESULT_AVG_SIZE);
                ptr_idx += V_RESULT_AVG_SIZE;
                break;
            }
            default:
            {
                printf("%s default case. i=%d\n", __func__, (int)i);
                break;
            }
            }
        }
    }
    // printf("ptr=0x%p\n", ptr);

    // print_array(ptr, size, "0x%02x");
    return 0;
}
/**
 * 功能: 校验数据类型(支持64bit数据)
 * 参数:
 *   - vp: 向量参数结构体
 * 返回值: bool true:校验成功 false:校验失败
 */
bool check_data_type_64(const VectorParam &vp)
{
    if (V_IN_TYPE_16 != vp.in1_type && V_IN_TYPE_32 != vp.in1_type && V_IN_TYPE_64 != vp.in1_type)
    {
        printf("%s: input type is invalid! type: %d\n", __func__, vp.in1_type);
        return false;
    }
    if (V_IN2_ENABLE == vp.in2_en)
    {
        if (V_IN_TYPE_16 != vp.in2_type && V_IN_TYPE_32 != vp.in2_type && V_IN_TYPE_64 != vp.in2_type)
        {
            printf("%s: input type is invalid! type: %d\n", __func__, vp.in2_type);
            return false;
        }
    }
    if (V_OUT_TYPE_16 != vp.out_type && V_OUT_TYPE_32 != vp.out_type)
    {
        printf("%s: output type is invalid! type: %d\n", __func__, vp.out_type);
        return false;
    }
    // 有求和的情况下,输出类型必须为32bit
    if (GET_BIT(vp.cmd_mode, 3) && V_OUT_TYPE_32 != vp.out_type)
    {
        printf("%s: cmd_mode conflicts with output type! cmd_mode: %d, type: %d\n", __func__, vp.cmd_mode, vp.out_type);
        return false;
    }
    // 输入类型有64bit的,输出必须8字节对齐,否则4字节对齐
    if (V_IN_TYPE_64 == vp.in1_type || (V_IN2_ENABLE == vp.in2_en && V_IN_TYPE_64 == vp.in2_type))
    {
        if (static_cast<int32_t>(pow(2, vp.out_type)) * 2 * vp.num % 8)
        {
            printf("%s: output must be 8-byte aligned!\n", __func__);
            return false;
        }
    }
    else
    {
        if (static_cast<int32_t>(pow(2, vp.out_type)) * 2 * vp.num % 4)
        {
            printf("%s: output must be 4-byte aligned!\n", __func__);
            return false;
        }
    }

    return true;
}
/**
 * 功能: 校验数据类型
 * 参数:
 *   - vp: 向量参数结构体
 * 返回值: bool true:校验成功 false:校验失败
 */
bool check_data_type(const VectorParam &vp)
{
    if (V_IN_TYPE_16 != vp.in1_type && V_IN_TYPE_32 != vp.in1_type)
    {
        printf("%s: input type is invalid! type: %d\n", __func__, vp.in1_type);
        return false;
    }
    if (V_IN2_ENABLE == vp.in2_en)
    {
        if (V_IN_TYPE_16 != vp.in2_type && V_IN_TYPE_32 != vp.in2_type)
        {
            printf("%s: input type is invalid! type: %d\n", __func__, vp.in2_type);
            return false;
        }
    }
    if (V_OUT_TYPE_16 != vp.out_type && V_OUT_TYPE_32 != vp.out_type)
    {
        printf("%s: output type is invalid! type: %d\n", __func__, vp.out_type);
        return false;
    }
    // 有求和的情况下,输出类型必须为32bit
    if (GET_BIT(vp.cmd_mode, 3) && V_OUT_TYPE_32 != vp.out_type)
    {
        printf("%s: cmd_mode conflicts with output type! cmd_mode: %d, type: %d\n", __func__, vp.cmd_mode, vp.out_type);
        return false;
    }
    // 输出必须4字节对齐
    if (static_cast<int32_t>(pow(2, vp.out_type)) * 2 * vp.num % 4)
    {
        printf("%s: output must be 4-byte aligned!\n", __func__);
        return false;
    }

    return true;
}
/**
 * 功能: 将中间数据转换为计算结果数据
 * 参数:
 *   - num: 循环次数,数据个数
 *   - mid_data_type: 中间数据类型
 *   - out_type: 输出数据类型
 *   - mid_data_ptr: 中间数据指针
 *   - calc_data: 计算结果数据指针的指针
 * 返回值: 计算结果数据的大小
 */
int32_t mid_data_conv_calc_data(int32_t num, int8_t mid_data_type, int8_t out_type, const int8_t *mid_data_ptr, int8_t *calc_data_ptr)
{
    // 计算的中间结果===>计算输出结果
    if (V_IN_TYPE_16 == mid_data_type)
    {
        if (V_OUT_TYPE_16 == out_type)
            read_data((int16_t *)mid_data_ptr, (int16_t *)calc_data_ptr, num);
        else if (V_OUT_TYPE_32 == out_type)
            read_data((int16_t *)mid_data_ptr, (int32_t *)calc_data_ptr, num);
    }
    else if (V_IN_TYPE_32 == mid_data_type)
    {
        if (V_OUT_TYPE_16 == out_type)
            read_data((int32_t *)mid_data_ptr, (int16_t *)calc_data_ptr, num);
        else if (V_OUT_TYPE_32 == out_type)
            read_data((int32_t *)mid_data_ptr, (int32_t *)calc_data_ptr, num);
    }
    else if (V_IN_TYPE_64 == mid_data_type)
    {
        if (V_OUT_TYPE_16 == out_type)
            read_data((int64_t *)mid_data_ptr, (int16_t *)calc_data_ptr, num);
        else if (V_OUT_TYPE_32 == out_type)
            read_data((int64_t *)mid_data_ptr, (int32_t *)calc_data_ptr, num);
    }
    return 0;
}

/**
 * 功能: 复制向量
 * 参数:
 *   - vp: 向量参数结构体
 * 返回值: 结果大小
 */
int32_t v_copy(VectorParam &vp)
{
    /** 1.输入输出数据类型校验 */
    if (false == check_data_type(vp))
    {
        return -1;
    }

    /** 3.中间结果转计算输出结果*/
    int32_t calc_data_size = static_cast<int32_t>(pow(2, vp.out_type)) * 2 * vp.num; // 计算的结果
    int8_t *calc_data_ptr = (int8_t *)malloc(calc_data_size);
    if (!calc_data_ptr)
    {
        printf("%s calc_data_ptr malloc failed!\n", __func__);
        return -1;
    }
    memset(calc_data_ptr, 0, calc_data_size);
    mid_data_conv_calc_data(vp.num, vp.in1_type, vp.out_type, (int8_t *)vp.adr_in1, calc_data_ptr);

    /** 4.计算和、最大值、最小值、均值*/
    int64_t sum = 0;
    int32_t mean = 0, max = INT32_MAX, min = INT32_MIN;
    for (size_t i = 0; i < vp.num; i++)
    {
        int32_t data = 0;
        if (V_OUT_TYPE_16 == vp.out_type)
            data = *(int16_t *)(calc_data_ptr + i * 2);
        else if (V_OUT_TYPE_32 == vp.out_type)
            data = *(int32_t *)(calc_data_ptr + i * 4);

        sum += data;
        if (data > max)
            max = data;
        if (data < min)
            min = data;
    }
    mean = static_cast<int32_t>(floor(static_cast<double>(sum) / vp.num * pow(2, 21)) / pow(2, 21));
    // set_bits(sum, 44, 63, 0xFFFFF1);

    /** 5.临时存储结果*/
    VectorResult result = {calc_data_ptr, calc_data_size, sum, max, min, mean};
    /************************************************************************/

    /** 6.结果保存 */
    get_vector_result(vp.cmd_mode, result, (int8_t *)vp.adr_out);
    FREE(calc_data_ptr);

    // printf("adr_out=0x%p\n", vp.adr_out);
    // print_array((uint8_t *)vp.adr_out, vp.out_size, "0x%02x");

    return 0;
}
/**
 * 功能: 取绝对值
 * 参数:
 *   - vp: 向量参数结构体
 * 返回值: 结果大小
 */
int32_t v_abs(VectorParam &vp)
{
    /** 1.输入输出数据类型校验 */
    if (false == check_data_type(vp))
    {
        return -1;
    }

    /** 2.计算中间结果 */
    for (size_t i = 0; i < vp.num; i++)
    {
        if (V_IN_TYPE_16 == vp.in1_type)
        {
            int16_t *ptr = (int16_t *)vp.adr_in1;
            *(ptr + i) = u31_abs(*(ptr + i));
        }
        else
        {
            int32_t *ptr = (int32_t *)vp.adr_in1;
            *(ptr + i) = u31_abs(*(ptr + i));
        }
    }

    /** 3.中间结果转计算输出结果*/
    int32_t calc_data_size = static_cast<int32_t>(pow(2, vp.out_type)) * 2 * vp.num; // 计算的结果
    int8_t *calc_data_ptr = (int8_t *)malloc(calc_data_size);
    if (!calc_data_ptr)
    {
        printf("%s calc_data_ptr malloc failed!\n", __func__);
        return -1;
    }
    memset(calc_data_ptr, 0, calc_data_size);
    mid_data_conv_calc_data(vp.num, vp.in1_type, vp.out_type, (int8_t *)vp.adr_in1, calc_data_ptr);

    /** 4.计算和、最大值、最小值、均值*/
    int64_t sum = 0;
    int32_t mean = 0, max = INT32_MAX, min = INT32_MIN;
    for (size_t i = 0; i < vp.num; i++)
    {
        int32_t data = 0;
        if (V_OUT_TYPE_16 == vp.out_type)
            data = *(int16_t *)(calc_data_ptr + i * 2);
        else if (V_OUT_TYPE_32 == vp.out_type)
            data = *(int32_t *)(calc_data_ptr + i * 4);

        sum += data;
        if (data > max)
            max = data;
        if (data < min)
            min = data;
    }
    mean = static_cast<int32_t>(floor(static_cast<double>(sum) / vp.num * pow(2, 21)) / pow(2, 21));
    // set_bits(sum, 44, 63, 0xFFFFF1);

    /** 5.临时存储结果*/
    VectorResult result = {calc_data_ptr, calc_data_size, sum, max, min, mean};
    /************************************************************************/

    /** 6.结果保存 */
    get_vector_result(vp.cmd_mode, result, (int8_t *)vp.adr_out);
    FREE(calc_data_ptr);
    // printf("adr_out=0x%p\n", vp.adr_out);
    // print_array((uint8_t *)vp.adr_out, vp.out_size, "0x%02x");

    return 0;
}

int32_t v_add(VectorParam &vp)
{
    /** 1.输入输出数据类型校验 */
    if (false == check_data_type(vp))
    {
        return -1;
    }

    /** 2.计算中间结果 */
    size_t mid_data_type_size = static_cast<int32_t>(pow(2, std::max(vp.in1_type, vp.in2_type) - 1) * 2); // 计算的中间结果
    int32_t mid_data_size = mid_data_type_size * vp.num;
    int8_t *mid_data_ptr = (int8_t *)malloc(mid_data_size);
    if (!mid_data_ptr)
    {
        printf("%s mid_data_ptr malloc failed!\n", __func__);
        return -1;
    }
    memset(mid_data_ptr, 0, mid_data_size);

    size_t in1_type_size = static_cast<int32_t>(pow(2, vp.in1_type - 1) * 2);
    size_t in2_type_size = static_cast<int32_t>(pow(2, vp.in2_type - 1) * 2);

    for (size_t i = 0; i < vp.num; i++)
    {
        int32_t in1 = 0, in2 = 0; // 用最大的类型取数据,兼容2种数据类型
        int64_t sum = 0;
        auto return_data = [](int8_t type, int8_t *addr) -> int32_t
        {
            if (V_IN_TYPE_16 == type)
            {
                return *(int16_t *)addr;
            }
            else // if (V_IN_TYPE_32 == type)
            {
                return *(int32_t *)addr;
            }
        };
        in1 = return_data(vp.in1_type, (int8_t *)vp.adr_in1 + i * in1_type_size);
        in2 = return_data(vp.in2_type, (int8_t *)vp.adr_in2 + i * in2_type_size);
        sum = in1 + in2;
        auto limits_sum = [](int8_t type, int64_t sum) -> int32_t
        {
            if (V_IN_TYPE_16 == type)
            {
                return get_limits<int16_t>(sum);
            }
            else // if (V_IN_TYPE_32 == type)
            {
                return get_limits<int32_t>(sum);
            }
        };
        // 限制和的范围
        int32_t sum32 = limits_sum(std::max(vp.in1_type, vp.in2_type), sum);
        memcpy(mid_data_ptr + i * mid_data_type_size, &sum32, mid_data_type_size);
    }

    /** 3.中间结果转计算输出结果*/
    int32_t calc_data_size = static_cast<int32_t>(pow(2, vp.out_type)) * 2 * vp.num; // 计算的结果
    int8_t *calc_data_ptr = (int8_t *)malloc(calc_data_size);
    if (!calc_data_ptr)
    {
        printf("%s calc_data_ptr malloc failed!\n", __func__);
        FREE(mid_data_ptr);
        return -1;
    }
    memset(calc_data_ptr, 0, calc_data_size);
    mid_data_conv_calc_data(vp.num, std::max(vp.in1_type, vp.in2_type), vp.out_type, mid_data_ptr, calc_data_ptr);

    /** 4.计算和、最大值、最小值、均值*/
    int64_t sum = 0;
    int32_t mean = 0, max = INT32_MAX, min = INT32_MIN;
    for (size_t i = 0; i < vp.num; i++)
    {
        int32_t data = 0;
        if (V_OUT_TYPE_16 == vp.out_type)
            data = *(int16_t *)(calc_data_ptr + i * 2);
        else if (V_OUT_TYPE_32 == vp.out_type)
            data = *(int32_t *)(calc_data_ptr + i * 4);

        sum += data;
        if (data > max)
            max = data;
        if (data < min)
            min = data;
    }
    mean = static_cast<int32_t>(floor(static_cast<double>(sum) / vp.num * pow(2, 21)) / pow(2, 21));
    // set_bits(sum, 44, 63, 0xFFFFF1);

    /** 5.临时存储结果*/
    VectorResult result = {calc_data_ptr, calc_data_size, sum, max, min, mean};
    /************************************************************************/

    /** 6.结果保存 */
    get_vector_result(vp.cmd_mode, result, (int8_t *)vp.adr_out);
    FREE(calc_data_ptr);
    FREE(mid_data_ptr);
    // printf("adr_out=0x%p\n", vp.adr_out);
    // print_array((uint8_t *)vp.adr_out, vp.out_size, "0x%02x");

    return 0;
}

int32_t v_sub(VectorParam &vp)
{
    /** 1.输入输出数据类型校验 */
    if (false == check_data_type(vp))
    {
        return -1;
    }

    /** 2.计算中间结果 */
    size_t mid_data_type_size = static_cast<int32_t>(pow(2, std::max(vp.in1_type, vp.in2_type) - 1) * 2); // 计算的中间结果
    int32_t mid_data_size = mid_data_type_size * vp.num;
    int8_t *mid_data_ptr = (int8_t *)malloc(mid_data_size);
    if (!mid_data_ptr)
    {
        printf("%s mid_data_ptr malloc failed!\n", __func__);
        return -1;
    }
    memset(mid_data_ptr, 0, mid_data_size);

    size_t in1_type_size = static_cast<int32_t>(pow(2, vp.in1_type - 1) * 2);
    size_t in2_type_size = static_cast<int32_t>(pow(2, vp.in2_type - 1) * 2);

    for (size_t i = 0; i < vp.num; i++)
    {
        int32_t in1 = 0, in2 = 0; // 用最大的类型取数据,兼容2种数据类型
        int64_t diff = 0;
        auto return_data = [](int8_t type, int8_t *addr) -> int32_t
        {
            if (V_IN_TYPE_16 == type)
            {
                return *(int16_t *)addr;
            }
            else // if (V_IN_TYPE_32 == type)
            {
                return *(int32_t *)addr;
            }
        };
        in1 = return_data(vp.in1_type, (int8_t *)vp.adr_in1 + i * in1_type_size);
        in2 = return_data(vp.in2_type, (int8_t *)vp.adr_in2 + i * in2_type_size);
        diff = in1 - in2;
        auto limits_diff = [](int8_t type, int64_t diff) -> int32_t
        {
            if (V_IN_TYPE_16 == type)
            {
                return get_limits<int16_t>(diff);
            }
            else // if (V_IN_TYPE_32 == type)
            {
                return get_limits<int32_t>(diff);
            }
        };
        // 限制和的范围
        int32_t diff32 = limits_diff(std::max(vp.in1_type, vp.in2_type), diff);
        memcpy(mid_data_ptr + i * mid_data_type_size, &diff32, mid_data_type_size);
    }

    /** 3.中间结果转计算输出结果*/
    int32_t calc_data_size = static_cast<int32_t>(pow(2, vp.out_type)) * 2 * vp.num; // 计算的结果
    int8_t *calc_data_ptr = (int8_t *)malloc(calc_data_size);
    if (!calc_data_ptr)
    {
        printf("%s calc_data_ptr malloc failed!\n", __func__);
        FREE(mid_data_ptr);
        return -1;
    }
    memset(calc_data_ptr, 0, calc_data_size);
    mid_data_conv_calc_data(vp.num, std::max(vp.in1_type, vp.in2_type), vp.out_type, mid_data_ptr, calc_data_ptr);

    /** 4.计算和、最大值、最小值、均值*/
    int64_t sum = 0;
    int32_t mean = 0, max = INT32_MAX, min = INT32_MIN;
    for (size_t i = 0; i < vp.num; i++)
    {
        int32_t data = 0;
        if (V_OUT_TYPE_16 == vp.out_type)
            data = *(int16_t *)(calc_data_ptr + i * 2);
        else if (V_OUT_TYPE_32 == vp.out_type)
            data = *(int32_t *)(calc_data_ptr + i * 4);

        sum += data;
        if (data > max)
            max = data;
        if (data < min)
            min = data;
    }
    mean = static_cast<int32_t>(floor(static_cast<double>(sum) / vp.num * pow(2, 21)) / pow(2, 21));
    // set_bits(sum, 44, 63, 0xFFFFF1);

    /** 5.临时存储结果*/
    VectorResult result = {calc_data_ptr, calc_data_size, sum, max, min, mean};
    /************************************************************************/

    /** 6.结果保存 */
    get_vector_result(vp.cmd_mode, result, (int8_t *)vp.adr_out);
    FREE(calc_data_ptr);
    FREE(mid_data_ptr);
    // printf("adr_out=0x%p\n", vp.adr_out);
    // print_array((uint8_t *)vp.adr_out, vp.out_size, "0x%02x");

    return 0;
}

int32_t v_min(VectorParam &vp)
{
    /** 1.输入输出数据类型校验 */
    if (false == check_data_type(vp))
    {
        return -1;
    }
    if (V_NORMAL_MODE != vp.cmd_mode)
    {
        printf("%s only surport normal mode. cmd_mod=%d\n", __func__, vp.cmd_mode);
        return -1;
    }
    /** 2.计算中间结果 */
    size_t mid_data_type_size = static_cast<int32_t>(pow(2, std::max(vp.in1_type, vp.in2_type) - 1) * 2); // 计算的中间结果
    int32_t mid_data_size = mid_data_type_size * vp.num;
    int8_t *mid_data_ptr = (int8_t *)malloc(mid_data_size);
    if (!mid_data_ptr)
    {
        printf("%s mid_data_ptr malloc failed!\n", __func__);
        return -1;
    }
    memset(mid_data_ptr, 0, mid_data_size);

    size_t in1_type_size = static_cast<int32_t>(pow(2, vp.in1_type - 1) * 2);
    size_t in2_type_size = static_cast<int32_t>(pow(2, vp.in2_type - 1) * 2);
    for (size_t i = 0; i < vp.num; i++)
    {
        int32_t in1 = 0, in2 = 0; // 用最大的类型取数据,兼容3种数据类型
        int32_t min = 0;
        auto return_data = [](int8_t type, int8_t *addr) -> int32_t
        {
            if (V_IN_TYPE_16 == type)
            {
                return *(int16_t *)addr;
            }
            else // if (V_IN_TYPE_32 == type)
            {
                return *(int32_t *)addr;
            }
        };
        in1 = return_data(vp.in1_type, (int8_t *)vp.adr_in1 + i * in1_type_size);
        in2 = return_data(vp.in2_type, (int8_t *)vp.adr_in2 + i * in2_type_size);
        min = in1 < in2 ? in1 : in2;
        memcpy(mid_data_ptr + i * mid_data_type_size, &min, mid_data_type_size);
    }

    /** 3.中间结果转计算输出结果*/
    mid_data_conv_calc_data(vp.num, std::max(vp.in1_type, vp.in2_type), vp.out_type, mid_data_ptr, (int8_t *)vp.adr_out);
    FREE(mid_data_ptr);

    // printf("adr_out=0x%p\n", vp.adr_out);
    // print_array((uint8_t *)vp.adr_out, vp.out_size, "0x%02x");

    return 0;
}

int32_t v_max(VectorParam &vp)
{
    /** 1.输入输出数据类型校验 */
    if (false == check_data_type(vp))
    {
        return -1;
    }
    if (V_NORMAL_MODE != vp.cmd_mode)
    {
        printf("%s only surport normal mode. cmd_mod=%d\n", __func__, vp.cmd_mode);
        return -1;
    }

    /** 2.计算中间结果 */
    size_t mid_data_type_size = static_cast<int32_t>(pow(2, std::max(vp.in1_type, vp.in2_type) - 1) * 2); // 计算的中间结果 用比最大输入数据类型宽一倍的内存存储
    int32_t mid_data_size = mid_data_type_size * vp.num;
    int8_t *mid_data_ptr = (int8_t *)malloc(mid_data_size);
    if (!mid_data_ptr)
    {
        printf("%s mid_data_ptr malloc failed!\n", __func__);
        return -1;
    }
    memset(mid_data_ptr, 0, mid_data_size);

    size_t in1_type_size = static_cast<int32_t>(pow(2, vp.in1_type - 1) * 2);
    size_t in2_type_size = static_cast<int32_t>(pow(2, vp.in2_type - 1) * 2);

    for (size_t i = 0; i < vp.num; i++)
    {
        int32_t in1 = 0, in2 = 0; // 用最大的类型取数据,兼容3种数据类型
        int32_t max = 0;
        auto return_data = [](int8_t type, int8_t *addr) -> int32_t
        {
            if (V_IN_TYPE_16 == type)
            {
                return *(int16_t *)addr;
            }
            else // if (V_IN_TYPE_32 == type)
            {
                return *(int32_t *)addr;
            }
        };
        in1 = return_data(vp.in1_type, (int8_t *)vp.adr_in1 + i * in1_type_size);
        in2 = return_data(vp.in2_type, (int8_t *)vp.adr_in2 + i * in2_type_size);
        max = in1 < in2 ? in2 : in1;

        memcpy(mid_data_ptr + i * mid_data_type_size, &max, mid_data_type_size);
    }

    /** 3.中间结果转计算输出结果*/
    mid_data_conv_calc_data(vp.num, std::max(vp.in1_type, vp.in2_type), vp.out_type, mid_data_ptr, (int8_t *)vp.adr_out);
    FREE(mid_data_ptr);

    // printf("adr_out=0x%p\n", vp.adr_out);
    // print_array((uint8_t *)vp.adr_out, vp.out_size, "0x%02x");

    return 0;
}

int32_t v_conj(VectorParam &vp)
{
    /** 1.输入输出数据类型校验 */
    if (false == check_data_type(vp))
    {
        return -1;
    }
    if (V_NORMAL_MODE != vp.cmd_mode)
    {
        printf("%s only surport normal mode. cmd_mod=%d\n", __func__, vp.cmd_mode);
        return -1;
    }
    if (V_IN_TYPE_32 != vp.in1_type || V_OUT_TYPE_32 != vp.out_type)
    {
        printf("%s only support type 32bit input or output! in1_type=%d, out_type=%d\n", __func__, vp.in1_type, vp.out_type);
        return -1;
    }

    /** 2.计算中间结果 */
    int32_t *in_ptr = (int32_t *)(vp.adr_in1);
    int32_t *out_ptr = (int32_t *)(vp.adr_out);

    for (size_t i = 0; i < vp.num; i++)
    {
        int16_t real = *(in_ptr + i) / pow(2, 16);
        int16_t imag = *(in_ptr + i) & 0xff;
        if (std::numeric_limits<int16_t>::min() == imag)
            imag = std::numeric_limits<int16_t>::max();
        else
            imag = -imag;
        *(out_ptr + i) = (real << 16) + imag;
    }

    // printf("adr_out=0x%p\n", vp.adr_out);
    // print_array((uint8_t *)vp.adr_out, vp.out_size, "0x%02x");

    return 0;
}
int32_t v_i32f32(VectorParam &vp)
{
    /** 1.输入输出数据类型校验 */
    if (false == check_data_type(vp))
    {
        return -1;
    }
    if (V_NORMAL_MODE != vp.cmd_mode)
    {
        printf("%s only surport normal mode. cmd_mod=%d\n", __func__, vp.cmd_mode);
        return -1;
    }
    if (V_IN_TYPE_32 != vp.in1_type)
    {
        printf("%s only support type 32bit input! type=%d\n", __func__, vp.in1_type);
        return -1;
    }
    auto return_data = [](int8_t type, int8_t *addr) -> int32_t
    {
        if (V_IN_TYPE_16 == type)
        {
            return *(int16_t *)addr;
        }
        else // if (V_IN_TYPE_32 == type)
        {
            return *(int32_t *)addr;
        }
    };
    /** 2.计算中间结果 */
    float *f = (float *)vp.adr_out;
    size_t in1_type_size = static_cast<int32_t>(pow(2, vp.in1_type - 1) * 2);
    size_t in2_type_size = static_cast<int32_t>(pow(2, vp.in2_type - 1) * 2);
    for (size_t i = 0; i < vp.num; i++)
    {
        int32_t in1 = return_data(vp.in1_type, (int8_t *)vp.adr_in1 + i * in1_type_size);
        int32_t in2 = return_data(vp.in2_type, (int8_t *)vp.adr_in2 + i * in2_type_size);
        *(f + i) = static_cast<float>(in1 / pow(2, 31 - in2));
    }

    /** 3.中间结果转计算输出结果*/
    // printf("adr_out=0x%p\n", vp.adr_out);
    // print_array((uint8_t *)vp.adr_out, vp.out_size, "0x%02x");

    return 0;
}

int32_t v_mix(VectorParam &vp)
{
    /** 1.输入输出数据类型校验 */
    if (false == check_data_type(vp))
    {
        return -1;
    }
    if (V_NORMAL_MODE != vp.cmd_mode)
    {
        printf("%s only surport normal mode. cmd_mod=%d\n", __func__, vp.cmd_mode);
        return -1;
    }
    if (V_IN_TYPE_32 != vp.in1_type || V_IN_TYPE_32 != vp.in2_type || V_OUT_TYPE_32 != vp.out_type)
    {
        printf("%s only support type 32bit input or output! in1_type=%d, in2_type=%d out_type=%d\n", __func__, vp.in1_type, vp.in2_type, vp.out_type);
        return -1;
    }
    int32_t *in1_ptr = (int32_t *)(vp.adr_in1);
    int32_t *in2_ptr = (int32_t *)(vp.adr_in2);
    int32_t *out_ptr = (int32_t *)(vp.adr_out);

    for (size_t i = 0; i < vp.num; i++)
    {
        int16_t real1 = *(in1_ptr + i) / pow(2, 16);
        int16_t imag1 = *(in1_ptr + i) & 0xff;
        int16_t real2 = *(in2_ptr + i) / pow(2, 16);
        int16_t imag2 = *(in2_ptr + i) & 0xff;
        int16_t real = (real1 + real2) / 2;
        int16_t imag = (imag1 + imag2) / 2;
        *(out_ptr + i) = (real << 16) + imag;
    }
    return 0;
}

int32_t v_energy(VectorParam &vp)
{
    /** 1.输入输出数据类型校验 */
    if (false == check_data_type(vp))
    {
        return -1;
    }

    if (V_IN_TYPE_32 != vp.in1_type || V_OUT_TYPE_32 != vp.out_type)
    {
        printf("%s only support type 32bit input or output! in1_type=%d, out_type=%d\n", __func__, vp.in1_type, vp.out_type);
        return -1;
    }

    /** 2.计算中间结果 */
    int32_t *in_ptr = (int32_t *)(vp.adr_in1);
    int32_t *out_ptr = (int32_t *)(vp.adr_out);

    for (size_t i = 0; i < vp.num; i++)
    {
        int16_t real = *(in_ptr + i) / pow(2, 16);
        int16_t imag = *(in_ptr + i) & 0xff;

        int64_t energy = (pow(real, 2) + pow(imag, 2)) * 2;
        if (energy < std::numeric_limits<int32_t>::min())
        {
            energy = std::numeric_limits<int32_t>::min();
            SET_BIT(vp.overflow_flag, 13);
        }
        else if (energy > std::numeric_limits<int32_t>::max())
        {
            energy = std::numeric_limits<int32_t>::max();
            SET_BIT(vp.overflow_flag, 13);
        }
        *(out_ptr + i) = static_cast<int32_t>(energy);
    }

    /** 4.计算和、最大值、最小值、均值*/
    int64_t sum = 0;
    int32_t mean = 0, max = INT32_MAX, min = INT32_MIN;
    for (size_t i = 0; i < vp.num; i++)
    {
        int32_t data = *(out_ptr + i);
        sum += data;
        if (data > max)
            max = data;
        if (data < min)
            min = data;
    }
    mean = static_cast<int32_t>(floor(static_cast<double>(sum) / vp.num * pow(2, 21)) / pow(2, 21));
    // set_bits(sum, 44, 63, 0xFFFFF1);

    /** 5.临时存储结果*/
    VectorResult result = {vp.adr_out, vp.out_size, sum, max, min, mean};
    /************************************************************************/

    /** 6.结果保存 */
    get_vector_result(vp.cmd_mode, result, (int8_t *)vp.adr_out);
    // printf("adr_out=0x%p\n", vp.adr_out);
    // print_array((uint8_t *)vp.adr_out, vp.out_size, "0x%02x");

    return 0;
}
int32_t v_add_c(VectorParam &vp)
{
    /** 1.输入输出数据类型校验 */
    if (false == check_data_type(vp))
    {
        return -1;
    }

    if (V_IN_TYPE_32 != vp.in1_type || V_IN_TYPE_32 != vp.in2_type || V_OUT_TYPE_32 != vp.out_type)
    {
        printf("%s only support type 32bit input or output! in1_type=%d, in2_type=%d out_type=%d\n", __func__, vp.in1_type, vp.in2_type, vp.out_type);
        return -1;
    }
    int32_t *in1_ptr = (int32_t *)(vp.adr_in1);
    int32_t *in2_ptr = (int32_t *)(vp.adr_in2);
    int32_t *out_ptr = (int32_t *)(vp.adr_out);

    for (size_t i = 0; i < vp.num; i++)
    {
        int16_t real1 = *(in1_ptr + i) / pow(2, 16);
        int16_t imag1 = *(in1_ptr + i) & 0xff;
        int16_t real2 = *(in2_ptr + i) / pow(2, 16);
        int16_t imag2 = *(in2_ptr + i) & 0xff;
        int32_t real = real1 + real2;
        if (real < std::numeric_limits<int16_t>::min())
        {
            real = std::numeric_limits<int16_t>::min();
            SET_BIT(vp.overflow_flag, 10);
        }
        else if (real > std::numeric_limits<int16_t>::max())
        {
            real = std::numeric_limits<int16_t>::max();
            SET_BIT(vp.overflow_flag, 10);
        }
        int32_t imag = imag1 + imag2;
        if (imag < std::numeric_limits<int16_t>::min())
        {
            imag = std::numeric_limits<int16_t>::min();
            SET_BIT(vp.overflow_flag, 10);
        }
        else if (imag > std::numeric_limits<int16_t>::max())
        {
            imag = std::numeric_limits<int16_t>::max();
            SET_BIT(vp.overflow_flag, 10);
        }
        *(out_ptr + i) = (real << 16) + imag;
    }
    /** 4.计算和、最大值、最小值、均值*/
    int64_t sum = 0;
    int32_t mean = 0, max = INT32_MAX, min = INT32_MIN;
    for (size_t i = 0; i < vp.num; i++)
    {
        int32_t data = *(out_ptr + i);
        sum += data;
        if (data > max)
            max = data;
        if (data < min)
            min = data;
    }
    mean = static_cast<int32_t>(floor(static_cast<double>(sum) / vp.num * pow(2, 21)) / pow(2, 21));
    // set_bits(sum, 44, 63, 0xFFFFF1);

    /** 5.临时存储结果*/
    VectorResult result = {vp.adr_out, vp.out_size, sum, max, min, mean};
    /************************************************************************/

    /** 6.结果保存 */
    get_vector_result(vp.cmd_mode, result, (int8_t *)vp.adr_out);
    return 0;
}

int32_t v_sub_c(VectorParam &vp)
{
    /** 1.输入输出数据类型校验 */
    if (false == check_data_type(vp))
    {
        return -1;
    }
    if (V_IN_TYPE_32 != vp.in1_type || V_IN_TYPE_32 != vp.in2_type || V_OUT_TYPE_32 != vp.out_type)
    {
        printf("%s only support type 32bit input or output! in1_type=%d, in2_type=%d out_type=%d\n", __func__, vp.in1_type, vp.in2_type, vp.out_type);
        return -1;
    }
    int32_t *in1_ptr = (int32_t *)(vp.adr_in1);
    int32_t *in2_ptr = (int32_t *)(vp.adr_in2);
    int32_t *out_ptr = (int32_t *)(vp.adr_out);

    for (size_t i = 0; i < vp.num; i++)
    {
        int16_t real1 = *(in1_ptr + i) / pow(2, 16);
        int16_t imag1 = *(in1_ptr + i) & 0xff;
        int16_t real2 = *(in2_ptr + i) / pow(2, 16);
        int16_t imag2 = *(in2_ptr + i) & 0xff;
        int32_t real = real1 - real2;
        if (real < std::numeric_limits<int16_t>::min())
        {
            real = std::numeric_limits<int16_t>::min();
            SET_BIT(vp.overflow_flag, 9);
        }
        else if (real > std::numeric_limits<int16_t>::max())
        {
            real = std::numeric_limits<int16_t>::max();
            SET_BIT(vp.overflow_flag, 9);
        }
        int32_t imag = imag1 - imag2;
        if (imag < std::numeric_limits<int16_t>::min())
        {
            imag = std::numeric_limits<int16_t>::min();
            SET_BIT(vp.overflow_flag, 9);
        }
        else if (imag > std::numeric_limits<int16_t>::max())
        {
            imag = std::numeric_limits<int16_t>::max();
            SET_BIT(vp.overflow_flag, 9);
        }
        *(out_ptr + i) = (real << 16) + imag;
    }
    /** 4.计算和、最大值、最小值、均值*/
    int64_t sum = 0;
    int32_t mean = 0, max = INT32_MAX, min = INT32_MIN;
    for (size_t i = 0; i < vp.num; i++)
    {
        int32_t data = *(out_ptr + i);
        sum += data;
        if (data > max)
            max = data;
        if (data < min)
            min = data;
    }
    mean = static_cast<int32_t>(floor(static_cast<double>(sum) / vp.num * pow(2, 21)) / pow(2, 21));
    // set_bits(sum, 44, 63, 0xFFFFF1);

    /** 5.临时存储结果*/
    VectorResult result = {vp.adr_out, vp.out_size, sum, max, min, mean};
    /************************************************************************/

    /** 6.结果保存 */
    get_vector_result(vp.cmd_mode, result, (int8_t *)vp.adr_out);
    return 0;
}
int32_t v_sqrt(VectorParam &vp)
{
    /** 1.输入输出数据类型校验 */
    if (false == check_data_type(vp))
    {
        return -1;
    }
    if (V_NORMAL_MODE != vp.cmd_mode)
    {
        printf("%s only surport normal mode. cmd_mod=%d\n", __func__, vp.cmd_mode);
        return -1;
    }

    int32_t *in_ptr = (int32_t *)vp.adr_in1;
    int32_t *out_ptr = (int32_t *)vp.adr_out;
    for (size_t i = 0; i < vp.num; i++)
    {
        int32_t data = *(in_ptr + i);
        if (V_IN_TYPE_16 == vp.in1_type)
            data *= pow(2, 16); // 16bit为m8p8 左移16位变成m8p24
        int32_t nHat = 0;
        int32_t b = 32768;
        int32_t bshft = 15;
        int32_t valid;

        for (int32_t i = 0; i < 16; ++i)
        {
            int32_t tmps = ((nHat * 2) + b);
            int32_t tmp = tmps << bshft;

            valid = (data >= tmp) ? 1 : 0;
            if (valid)
            {
                nHat += b;
                data -= tmp;
            }
            b >>= 1;
            bshft--;
        }
        if (V_OUT_TYPE_16 == vp.out_type)
            *(out_ptr + i) = static_cast<int16_t>(floor(nHat / pow(2, 16)));
        else
            *(out_ptr + i) = nHat;
    }
    return 0;
}
int32_t v_exp2_m16p16(VectorParam &vp)
{
    /** 1.输入输出数据类型校验 */
    if (false == check_data_type(vp))
    {
        return -1;
    }
    if (V_NORMAL_MODE != vp.cmd_mode)
    {
        printf("%s only surport normal mode. cmd_mod=%d\n", __func__, vp.cmd_mode);
        return -1;
    }

    size_t in1_type_size = static_cast<int32_t>(pow(2, vp.in1_type - 1) * 2);
    size_t out_type_size = static_cast<int32_t>(pow(2, vp.out_type) * 2);
    for (size_t i = 0; i < vp.num; i++)
    {
        auto return_data = [](int8_t type, int8_t *addr) -> int32_t
        {
            if (V_IN_TYPE_16 == type)
            {
                return *(int16_t *)addr;
            }
            else // if (V_IN_TYPE_32 == type)
            {
                return *(int32_t *)addr;
            }
        };
        int32_t data = return_data(vp.in1_type, (int8_t *)vp.adr_in1 + i * in1_type_size);
        if (V_IN_TYPE_16 == vp.in1_type)
            data *= pow(2, 16); // 16bit为m8p8 左移16位变成m8p24
        auto set_data = [](int8_t type, int8_t *addr, int32_t value) -> int32_t
        {
            if (V_IN_TYPE_16 == type)
            {
                *(int16_t *)addr = static_cast<int16_t>(floor(value / pow(2, 16))); // m8p8
            }
            else // if (V_IN_TYPE_32 == type)
            {
                *(int32_t *)addr = value; // m8p24
            }
            return 0;
        };
        if (data <= -16 * pow(2, 24))
        {
            set_data(vp.out_type, (int8_t *)vp.adr_out + i * out_type_size, 1);
            return 0;
        }
        if (15 * pow(2, 24) <= data)
        {
            set_data(vp.out_type, (int8_t *)vp.adr_out + i * out_type_size, INT32_MAX);
            return 0;
        }
        int32_t x = data;
        double ret = 0.0;
        int8_t integer = x >> 24;             // 取m8p24格式的整数部分
        int32_t remain_bin = x & 0xffffff;    // 取m8p24格式的小数部分二进制对应的10进制数
        int32_t index = (x & 0xf80000) >> 19; // 取m8p24格式的小数部分前5个二进制数对应的10进制数
        int64_t dat_integer, dat_remain, mul_dat;

        if (0x000000 != remain_bin) // 判断是不是整数
        {
            if (CHECK_BIT(x, 31)) // 判断符号
            {
                dat_integer = static_cast<int64_t>(std::floor(pow(2, integer) * pow(2, 16)));                           // p16  因为输入浮点数小于-16,但取出来的integer最小就可以等于-16,此处乘以2^16是保留整数精度
                dat_remain = exp_lut[index];                                                                            // p14
                mul_dat = static_cast<int64_t>(std::floor(static_cast<double>(dat_integer) * dat_remain / pow(2, 14))); // p16
            }
            else
            {
                dat_integer = static_cast<int64_t>(std::floor(pow(2, integer) * pow(2, 2))); // p2  此处乘以2^2,并不是为了保留精度,因为正数不需要,只是为了与小数一起凑成p16
                dat_remain = exp_lut[index];                                                 // p14
                mul_dat = static_cast<int64_t>(std::floor(static_cast<double>(dat_integer) * dat_remain));
            }
            ret = mul_dat; // m16p16
        }
        else
        {
            ret = pow(2, integer) * pow(2, 16); // m16p16
        }
        // printf("\tinteger=%d remain_bin=0x%08x index=%d dat_integer=%ld dat_remain=%ld mul_dat=%d ret=%lf\n", integer, remain_bin, index, dat_integer, dat_remain, mul_dat, ret);

        if (ret > INT32_MAX)
            ret = INT32_MAX;

        set_data(vp.out_type, (int8_t *)vp.adr_out + i * out_type_size, ret);
    }
    return 0;
}
int32_t v_sin(VectorParam &vp)
{
    /** 1.输入输出数据类型校验 */
    if (false == check_data_type(vp))
    {
        return -1;
    }
    if (V_NORMAL_MODE != vp.cmd_mode)
    {
        printf("%s only surport normal mode. cmd_mod=%d\n", __func__, vp.cmd_mode);
        return -1;
    }

    size_t in1_type_size = static_cast<int32_t>(pow(2, vp.in1_type - 1) * 2);
    size_t out_type_size = static_cast<int32_t>(pow(2, vp.out_type) * 2);
    for (size_t i = 0; i < vp.num; i++)
    {
        auto return_data = [](int8_t type, int8_t *addr) -> int32_t
        {
            if (V_IN_TYPE_16 == type)
            {
                return *(int16_t *)addr;
            }
            else // if (V_IN_TYPE_32 == type)
            {
                return *(int32_t *)addr;
            }
        };
        int32_t data = return_data(vp.in1_type, (int8_t *)vp.adr_in1 + i * in1_type_size);
        if (V_IN_TYPE_16 == vp.in1_type)
            data *= pow(2, 16);    // 16bit为m8p8 左移16位变成m8p24
        int32_t value = sin(data); // m1p31
        auto set_data = [](int8_t type, int8_t *addr, int32_t value) -> int32_t
        {
            if (V_IN_TYPE_16 == type)
            {
                *(int16_t *)addr = static_cast<int16_t>(floor(value / pow(2, 16))); // m1p15
            }
            else // if (V_IN_TYPE_32 == type)
            {
                *(int32_t *)addr = value; // m1p31
            }
            return 0;
        };
        set_data(vp.out_type, (int8_t *)vp.adr_out + i * out_type_size, value);
    }
    return 0;
}
int32_t v_cos(VectorParam &vp)
{
    /** 1.输入输出数据类型校验 */
    if (false == check_data_type(vp))
    {
        return -1;
    }
    if (V_NORMAL_MODE != vp.cmd_mode)
    {
        printf("%s only surport normal mode. cmd_mod=%d\n", __func__, vp.cmd_mode);
        return -1;
    }

    size_t in1_type_size = static_cast<int32_t>(pow(2, vp.in1_type - 1) * 2);
    size_t out_type_size = static_cast<int32_t>(pow(2, vp.out_type) * 2);
    for (size_t i = 0; i < vp.num; i++)
    {
        auto return_data = [](int8_t type, int8_t *addr) -> int32_t
        {
            if (V_IN_TYPE_16 == type)
            {
                return *(int16_t *)addr;
            }
            else // if (V_IN_TYPE_32 == type)
            {
                return *(int32_t *)addr;
            }
        };
        int32_t data = return_data(vp.in1_type, (int8_t *)vp.adr_in1 + i * in1_type_size);
        if (V_IN_TYPE_16 == vp.in1_type)
            data *= pow(2, 16);    // 16bit为m8p8 左移16位变成m8p24
        int32_t value = cos(data); // m1p31
        auto set_data = [](int8_t type, int8_t *addr, int32_t value) -> int32_t
        {
            if (V_IN_TYPE_16 == type)
            {
                *(int16_t *)addr = static_cast<int16_t>(floor(value / pow(2, 16))); // m1p15
            }
            else // if (V_IN_TYPE_32 == type)
            {
                *(int32_t *)addr = value; // m1p31
            }
            return 0;
        };
        set_data(vp.out_type, (int8_t *)vp.adr_out + i * out_type_size, value);
    }
    return 0;
}

int32_t v_polarrect(VectorParam &vp)
{
    /** 1.输入输出数据类型校验 */
    if (false == check_data_type(vp))
    {
        return -1;
    }

    if (V_IN_TYPE_32 != vp.in1_type || V_OUT_TYPE_32 != vp.out_type)
    {
        printf("%s only support type 32bit input or output! in1_type=%d, out_type=%d\n", __func__, vp.in1_type, vp.out_type);
        return -1;
    }

    /** 2.计算中间结果 */
    int32_t *in_ptr = (int32_t *)(vp.adr_in1);
    int32_t *out_ptr = (int32_t *)(vp.adr_out);

    for (size_t i = 0; i < vp.num; i++)
    {
        int16_t rho = *(in_ptr + i) / pow(2, 16);       // 取高字节 m8p8
        int16_t theta = *(in_ptr + i) & 0xff;           // m8p8
        int32_t norm_theta = floor(theta * pow(2, 16)); // m8p8 ===> m8p24

        int32_t sin_theta = sin(norm_theta);
        int32_t cos_theta = cos(norm_theta);

        int16_t x = static_cast<int16_t>(floor(rho * cos_theta / pow(2, 16))); // m1p15
        int16_t y = static_cast<int16_t>(floor(rho * sin_theta / pow(2, 16))); // m1p15

        *(out_ptr + i) = (x << 16) + y;
    }

    // printf("adr_out=0x%p\n", vp.adr_out);
    // print_array((uint8_t *)vp.adr_out, vp.out_size, "0x%02x");

    return 0;
}
